#ifndef CIPM_HPP
#define CIPM_HPP
#include <cstdio>
#include <ctime>
#include <iostream>
#include <sstream>
#include <string>

#include "common.h"
#include <opencv2/opencv.hpp>

using namespace std;
using namespace cv;

class CPerspective {
private:
  /* data */
public:
  CPerspective(/* args */);
  ~CPerspective();
  void PM(Mat &img, vector<Point2f> &ori_points, Mat &camera_matrix,
          Mat &dist_coeffs);
  bool findPartten(Mat &img);
  void InitHomography();
  void IPMpoint(Point2f &fv, Point2f &bv);
  vector<Point2f> mFoundPts;
  vector<Point2f> RunTimeVisiblePoints;
  int mChessBoardFlags;
  Size mBoardSize;
  Mat mHomography;
  bool mState;
  string camParasFile;
  float xmin,ymin,xmax,ymax,w,h=0;
};

CPerspective::CPerspective(/* args */) {
  mChessBoardFlags =
      CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE | CALIB_CB_FAST_CHECK;
  mBoardSize = Size(9, 6);
  mState = false;
  camParasFile = "calibration_params.xml";
}

CPerspective::~CPerspective() {}

void CPerspective::PM(Mat &img, vector<Point2f> &ori_points, Mat &camera_matrix,
                      Mat &dist_coeffs) {
  if (findPartten(img)) {
    undistortPoints(mFoundPts, mFoundPts, camera_matrix, dist_coeffs);
    mHomography = findHomography(mFoundPts, ori_points); // get homography

    Mat warped_image;
    warpPerspective(img, img, mHomography, Size(WIDTH, HEIGHT));

    FileStorage fs(camParasFile, FileStorage::WRITE);
    fs << "H2_inv" << mHomography;
    gHomographyInited = true;
    mState = true;
  }
}

void CPerspective::IPMpoint(Point2f &fv, Point2f &bv) {
  Mat product;
  Mat ipm_p = (Mat_<double>(3, 1) << fv.x, fv.y, 1);
  product = mHomography * ipm_p;
  bv.x = product.at<double>(0) / product.at<double>(2);
  bv.y = product.at<double>(1) / product.at<double>(2);
}

void CPerspective::InitHomography() {
  mHomography = Mat::eye(3, 3, CV_64F);
  RunTimeVisiblePoints.clear();

  FileStorage fs(camParasFile, FileStorage::READ); // Read the settings
  if (!fs.isOpened()) {
    cout << "Could not open the H file: \"" << camParasFile << "\"" << endl;
    gHomographyInited = false;
    return;
  } else {
    cout << "Successed to open the H file: \"" << camParasFile << "\"" << endl;
    fs["H2_inv"] >> mHomography;
    if (mHomography.empty() || mHomography.rows != 3 || mHomography.cols != 3) {
      cout << "Failed to read H2_inv matrix" << endl;
      gHomographyInited = false;
      fs.release();
      return;
    }
    fs["RunTimeVisiblePoints"] >> RunTimeVisiblePoints;
    if (RunTimeVisiblePoints.size() > 0){
      xmin=RunTimeVisiblePoints[0].x;
      xmax = xmin;
      ymin=RunTimeVisiblePoints[0].y;
      ymax = ymin;
      for (auto& pt : RunTimeVisiblePoints){
        if (pt.x<xmin) xmin = pt.x;
        if (pt.y<ymin) ymin = pt.y;
        if (pt.x>xmax) xmax = pt.x;
        if (pt.y>ymax) ymax = pt.y;
      }
    }
    ymin = max(0.0,ymin-10.0);
    w = xmax - xmin;
    h = ymax - ymin;
    cout << "xmin,ymin,w,h: " << xmin << "," << ymin << "," << w << "," << h << endl;
    gHomographyInited = true;
  }
  fs.release(); // close Settings file
}

bool CPerspective::findPartten(Mat &img) {
  bool found = false;
  found = findChessboardCorners(img, mBoardSize, mFoundPts, mChessBoardFlags);
  // Draw the corners.
  // drawChessboardCorners( img, mBoardSize, Mat(mFoundPts), found );
  // for(Point2f &p:mFoundPts)
  // {
  //     cout << p << endl;
  // }
  return found;
}
#endif
